


void inversion_force(const vector_type & ri,const vector_type & rj,const vector_type & rk,const vector_type & rl,
(void double*) u_expr(const double &),(void double*) du_expr(const double &),
real_type & u,
vector_type & fi,vector_type & fj,vector_type & fk,vector_type & fl)
{

    vector_type a = ri - rj;
    vector_type b = ri - rk;
    vector_type c = ri - rl;
    
//    abc = a.cross(b).dot(c);   // vegyes szorzat pozitiv abc > 0 ez a kriterium a ijkl sorbarendezesnel
    
    real_type ila = 1.0/a.norm();
    real_type ilb = 1.0/b.norm();
    real_type ilc = 1.0/c.norm();
    
    real_type aa = a.dot(a);
    real_type bb = b.dot(b);
    real_type cc = c.dot(c);
    
    real_type ab = a.dot(b);
    real_type ca = c.dot(a);
    real_type bc = b.dot(c);
        
    vector_type axb = a.cross(b);
    vector_type cxa = c.cross(a);
    vector_type bxc = b.cross(c);
    
    real_type abc = axb.dot(c);
    
    real_type ilaxb = 1.0/axb.norm();
    real_type ilcxa = 1.0/cxa.norm();
    real_type ilbxc = 1.0/bxc.norm();
  
    vector_type Alaxb = ilaxb * ( bb * a - ab * b );
    vector_type Blaxb = ilaxb * ( aa * b - ab * a );
    
    vector_type Alcxa = ilcxa * ( aa * c - ca * a );
    vector_type Clcxa = ilcxa * ( cc * a - ca * c );
    
    vector_type Blbxc = ilbxc * ( cc * b - bc * c );
    vector_type Clbxc = ilbxc * ( bb * c - bc * b );
    
    vector_type Aabc = bxc;
    vector_type Babc = cxa;
    vector_type Cabc = axb;
    
    vector_type Ailaxb = - ilaxb*ilaxb * Alaxb;
    vector_type Bilaxb = - ilaxb*ilaxb * Blaxb;
    vector_type Cilc   = - ilc*ilc*ilc * c;
    
    vector_type Cilcxa = - ilcxa*ilcxa * Clcxa;
    vector_type Ailcxa = - ilcxa*ilcxa * Alcxa;
    vector_type Bilb   = - ilb*ilb*ilb * b;
    
    vector_type Bilbxc = - ilbxc*ilbxc * Blbxc;
    vector_type Cilbxc = - ilbxc*ilbxc * Clbxc;
    vector_type Aila   = - ila*ila*ila * a;
    
    real_type divs = ilaxb*ilc + ilcxa*ilb + ilbxc*ila;
    
    vector_type Adivs = ilc*Ailaxb + ilb*Ailcxa + ilbxa*Aila;
    vector_type Bdivs = ilc*Bilaxb + ilcxa*Bilb + ila*Bilbxc;
    vector_type Cdivs = ilaxb*Cilc + ilb*Cilcxa + ils*Cilbxc;
        
    real_type cosabc = abc*divs;
    
    vector_type Acosabc = Aabc*divs + abc*Adivs;
    vector_type Bcosabc = Babc*divs + abc*Bdivs;
    vector_type Ccosabc = Cabc*divs + abc*Cdivs;

    real_type cosphi = abc*ilaxb*ilc;
    real_type phi = acos(cosphi);
    real_type isinphi = 1.0/sin(phi);
    real_type theta = 0.5*pi - phi;
    
    u = u_expr(theta);
    real_type du = du_expr(theta);
    
    fj = isinphi * du * Acosabc;
    fk = isinphi * du * Bcosabc;
    fl = isinphi * du * Ccosabc;
    
    fi = -fj-fk-fl;
}

double inversion_angle(const vector_type & ri,const vector_type & rj,const vector_type & rk,const vector_type & rl)
{
    cosphi = (ri - rj).cross(ri - rk).normalized().dot((ri - rl).normalized());
    return 0.5*pi - acos(cosphi);;    
}



void dihedral_force(const vector_type & ri,const vector_type & rj,const vector_type & rk,const vector_type & rl,
(void double*) u_expr(const double &),(void double*) du_expr(const double &),
real_type & u,
vector_type & fi,vector_type & fj,vector_type & fk,vector_type & fl)
{
    vector_type a = ri - rj;  vector_type b = rj - rk;  vector_type c = rk - rl;

    real_type aa = a.dot(a);  real_type bb = b.dot(b);  real_type cc = c.dot(c);
    real_type ab = a.dot(b);  real_type ac = a.dot(c);  real_type bc = b.dot(c);
    
    vector_type axb   = a.cross(b);       vector_type bxc   = b.cross(c); 

    real_type   ilaxb = 1.0/axb.norm();   real_type   ilbxc = 1.0/bxc.norm();
    
    //axbbxc = axb*bxc;
    real_type axbbxc = ab * bc - bb * ac;
    
    real_type inv = ilaxb*ilbxc;
    
    vector_type Aaxbbxc = b * bc - bb * c;
    vector_type Baxbbxc = a * bc + ab * c - 2.0 * b * ac;
    vector_type Caxbbxc = ab * b - bb * a;
    
    vector_type Alaxb = ilaxb * ( bb * a - ab * b );
    vector_type Blaxb = ilaxb * ( aa * b - ab * a );

    vector_type Blbxc = ilbxc * ( cc * b - bc * c );
    vector_type Clbxc = ilbxc * ( bb * c - bc * b );

    vector_type Ailaxb = - ilaxb*ilaxb * Alaxb;
    vector_type Bilaxb = - ilaxb*ilaxb * Blaxb;    
    
    vector_type Bilbxc = - ilbxc*ilbxc * Blbxc;
    vector_type Cilbxc = - ilbxc*ilbxc * Clbxc;
        
    vector_type Ainv = ilbxc * Ailaxb;
    vector_type Binv = Bilaxb * ilbxc + ilaxb * Bilbxc;
    vector_type Cinv = ilaxb * Cilbxc;
            
    real_type cosphi = axbbxc*inv;
    
    vector_type Acosphi = Aaxbbxc * inv + axbbxc * Ainv;
    vector_type Bcosphi = Baxbbxc * inv + axbbxc * Binv;
    vector_type Ccosphi = Caxbbxc * inv + axbbxc * Cinv;
    
    real_type phi = acos(cosphi);
    real_type isinphi = 1.0/sin(phi);
 
    u = u_expr(phi);
    real_type du = du_expr(phi); 
    
    fi = isinphi * du * Acosphi;
    fj = isinphi * du * (Bcosphi - Acosphi);
    fl = -isinphi * du * Ccosphi;
    
    fk = -fi-fj-fl

}

double dihedral_angle(const vector_type & ri,const vector_type & rj,const vector_type & rk,const vector_type & rl)
{
    cosphi = (ri - rj).cross(rj - rk).normalized().dot((rj - rk).cross(rk - rl).normalized());
    return acos(cosphi);    
}

